/*
 **********************************************************************
 * File name: matrix.h
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include <cmath>
#include <cstring>
#include "vector.h"
/* 
*/
namespace matrix
{
  
template<typename Type, size_t M>
class DiagMatrix;

template<typename Type, size_t M>
class Vector;

template<typename Type, size_t  M, size_t N>
class Matrix
{

public:
  
  Type data[M][N];

  Matrix()
  {
  }
  
  Matrix(const Type data0)
  {
    Type *ptr1 = (Type*)data;
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = data0;
    }
  }
  
  Matrix(const Type data0[][N])
  {
    memcpy(data, data0, sizeof(data));
  }

  Matrix(const Type *data0)
  {
    memcpy(data, data0, sizeof(data));
  }

  Matrix(const Matrix<Type, M, N> &vb)
  {
    memcpy(data, vb.data, sizeof(data));
  }

  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * M1 = M2
   */
  Matrix<Type, M, N> &operator=(const Matrix<Type, M, N> &vb)
  {
    if (this != &vb) {
      memcpy(data, vb.data, sizeof(data));
    }
    return (*this);
  }
  
  /*
   * M1 + M2
   */
  Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &vb) const
  {
    Matrix<Type, M, N> vr;
    
    Type *ptr1 = (Type*)vr.data;
    Type *ptr2 = (Type*)data;
    Type *ptr3 = (Type*)vb.data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = ptr2[i] + ptr3[i];
    }
    
    return vr;
  }
  
  /*
   * M1 += M2
   */
  void operator+=(const Matrix<Type, M, N> &vb)
  {
    Type *ptr1 = (Type*)data;
    Type *ptr2 = (Type*)vb.data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] += ptr2[i];
    }
  }
  /*
   * M1 + DM2
   */
  Matrix<Type, M, N> operator+(const DiagMatrix<Type, M> &vb) const
  {
    Matrix<Type, M, N> vr = *this;
    size_t min = M;
    if(M > N) {
      min = N;
    }
    
    for (size_t i = 0; i < min; i++) {
      vr.data[i][i] += vb.v[i];
    }
    
    return vr;
  }
  /*
   * M1 += DM2
   */
  void operator+=(const DiagMatrix<Type, M> &vb)
  {
    size_t min = M;
    if(M > N) {
      min = N;
    }
    
    for (size_t i = 0; i < min; i++) {
      this->data[i][i] += vb.v[i];
    }
  }
  
  
  /*
   * M1 - M2
   */
  Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &vb) const
  {
    Matrix<Type, M, N> vr;
    
    Type *ptr1 = (Type*)vr.data;
    Type *ptr2 = (Type*)data;
    Type *ptr3 = (Type*)vb.data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = ptr2[i] - ptr3[i];
    }
    
    return vr;
  }
  
  /*
   * M1 -= M2
   */
  void operator-=(const Matrix<Type, M, N> &vb)
  {
    Type *ptr1 = (Type*)data;
    Type *ptr2 = (Type*)vb.data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] -= ptr2[i];
    }
  }
  
  /*
   * M1 - DM2
   */
  Matrix<Type, M, N> operator-(const DiagMatrix<Type, M> &vb) const
  {
    Matrix<Type, M, N> vr = *this;
    size_t min = M;
    if(M > N) {
      min = N;
    }
    
    for (size_t i = 0; i < min; i++) {
      vr.data[i][i] -= vb.v[i];
    }
    
    return vr;
  }
  /*
   * M1 -= DM2
   */
  void operator-=(const DiagMatrix<Type, M> &vb)
  {
    size_t min = M;
    if(M > N) {
      min = N;
    }
    
    for (size_t i = 0; i < min; i++) {
      this->data[i][i] -= vb.v[i];
    }
  }
  
  /*
   * -M
   */
  Matrix<Type, M, N> operator-() const
  {
    Matrix<Type, M, N> vr;

    Type *ptr1 = (Type*)vr.data;
    Type *ptr2 = (Type*)data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = -ptr2[i];
    }

    return vr;
  }
  
  /*
   * M1 * M2
   */
  template<size_t P>
  Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &vb) const
  {
    Matrix<Type, M, P> vr;

    for (size_t i = 0; i < M; i++) {
      for (size_t k = 0; k < P; k++) {
        vr.data[i][k] = 0;
        for (size_t j = 0; j < N; j++) {
            vr.data[i][k] += data[i][j] * vb.data[j][k];
        }
      }
    }

    return vr;
  }
  
  /*
   * M1 * DM2
   */
  Matrix<Type, M, N> operator*(const DiagMatrix<Type, N> &vb) const
  {
    Matrix<Type, M, N> vr = *this;

    for (size_t i = 0; i < N; i++) {
      for (size_t j = 0; j < M; j++) {
        vr.data[j][i] *= vb.v[i];
      }
    }

    return vr;
  }
  
  /*
   * M1 *= DM2
   */
  void operator*=(const DiagMatrix<Type, N> &vb)
  {
    for (size_t i = 0; i < N; i++) {
      for (size_t j = 0; j < M; j++) {
        this->data[j][i] *= vb.v[i];
      }
    }
  }
  
  /*
   * M1 * V2
   */
  Vector<Type, M> operator*(const Vector<Type, N> &vb) const
  {
    Vector<Type, M> vr;

    for (size_t i = 0; i < M; i++) {
      vr.v[i] = 0;
      for (size_t j = 0; j < N; j++) {
        vr.v[i] += this->data[i][j] * vb.v[j];
      }
    }

    return vr;
  }
  
  /*
   * M1 * S2
   */
  Matrix<Type, M, N> operator*(Type scalar) const
  {
    Matrix<Type, M, N> vr;

    Type *ptr1 = (Type*)vr.data;
    Type *ptr2 = (Type*)data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = ptr2[i] * scalar;
    }
    
    return vr;
  }

  /*
   * M1 *= S2
   */
  void operator*=(Type scalar)
  {
    Type *ptr1 = (Type*)data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] *= scalar;
    }
  }

  /*
   * M1 / S2
   */
  Matrix<Type, M, N> operator/(Type scalar) const
  {
    Matrix<Type, M, N> vr;

    Type *ptr1 = (Type*)vr.data;
    Type *ptr2 = (Type*)data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] = ptr2[i] / scalar;
    }
    
    return vr;
  }

  /*
   * M1 /= S2
   */
  void operator/=(Type scalar)
  {
    Type *ptr1 = (Type*)data;
    
    for(size_t i=0; i<M*N; i++) {
      ptr1[i] /= scalar;
    }
  }
  
  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  Matrix<Type, N, M> transpose() const
  {
    Matrix<Type, N, M> vr;

    for (size_t i = 0; i < M; i++) {
      for (size_t j = 0; j < N; j++) {
        vr.data[j][i] = data[i][j];
      }
    }

    return vr;
  }
  
  inline Matrix<Type, N, M> T() const
  {
    return this->transpose();
  }

  template<size_t P, size_t Q>
  Matrix<Type, P, Q> slice(size_t x0, size_t y0) const
  {
      Matrix<Type, P, Q> vr(&(data[x0][y0]));
      return vr;
  }

  template<size_t P, size_t Q>
  void set(const Matrix<Type, P, Q> &m, size_t x0, size_t y0)
  {
      for (size_t i = 0; i < P; i++) {
          for (size_t j = 0; j < Q; j++) {
              data[i + x0][j + y0] = m.data[i][j];
          }
      }
  }

  void set_row(size_t i, const Matrix<Type, N, 1> &row)
  {
      for (size_t j = 0; j < N; j++) {
          data[i][j] = row.data[j][0];
      }
  }

  void set_col(size_t j, const Matrix<Type, M, 1> &col)
  {
      for (size_t i = 0; i < M; i++) {
          data[i][j] = col.data[i][0];
      }
  }

  void set_zero()
  {
      memset(data, 0, sizeof(data));
  }

  inline void zero()
  {
      set_zero();
  }

  void set_all(Type val)
  {
      for (size_t i = 0; i < M; i++) {
          for (size_t j = 0; j < N; j++) {
              data[i][j] = val;
          }
      }
  }

  void swap_rows(size_t a, size_t b)
  {
      if (a == b) {
          return;
      }

      for (size_t j = 0; j < N; j++) {
          Type tmp = data[a][j];
          data[a][j] = data[b][j];
          data[b][j] = tmp;
      }
  }

  void swap_cols(size_t a, size_t b)
  {
      if (a == b) {
          return;
      }

      for (size_t i = 0; i < M; i++) {
          Type tmp = data[i][a];
          data[i][a] = data[i][b];
          data[i][b] = tmp;
      }
  }

  Matrix<Type, M, N> abs() const
  {
      Matrix<Type, M, N> vr;
      for (size_t i=0; i<M; i++) {
          for (size_t j=0; j<N; j++) {
              vr.data[i][j] = Type(fabs(data[i][j]));
          }
      }
      return vr;
  }

  Type max() const
  {
      Type max_val = data[0][0];
      for (size_t i=0; i<M; i++) {
          for (size_t j=0; j<N; j++) {
              Type val = data[i][j];
              if (val > max_val) {
                  max_val = val;
              }
          }
      }
      return max_val;
  }

  Type min() const
  {
      Type min_val = data[0][0];
      for (size_t i=0; i<M; i++) {
          for (size_t j=0; j<N; j++) {
              Type val = data[i][j];
              if (val < min_val) {
                  min_val = val;
              }
          }
      }
      return min_val;
  }
  
  Type norm() const
  {
    Type vr = 0;
    Type *ptr1 = (Type*)data;
    for(size_t i = 0; i < M*N; i++) {
      vr += ptr1[i]*ptr1[i];
    }
    return sqrtf(vr);
  }
  
  inline Matrix<Type, M, N> normalized() const
  {
    return  *this / norm();
  }
  
  inline Type normalize()
  {
    Type tmp = norm();
    *this /= norm();
    return tmp;
  }
};



/*---------------------------------------------*
 *               SquareMatrix                  *
 *---------------------------------------------*/
template<typename Type, size_t  M>
class SquareMatrix : public Matrix<Type, M, M>
{
public:
  
  SquareMatrix() :
    Matrix<Type, M, M>()
  {}

  SquareMatrix(Type data0) :
    Matrix<Type, M, M>(data0)
  {}
    
  SquareMatrix(const Type data0[][M]) :
    Matrix<Type, M, M>(data0)
  {}
  
  SquareMatrix(const Type *data0) :
    Matrix<Type, M, M>(data0)
  {}

  SquareMatrix(const Matrix<Type, M, M> &vb) :
    Matrix<Type, M, M>(vb)
  {}
  
  SquareMatrix(const SquareMatrix<Type, M> &vb) :
    Matrix<Type, M, M>(vb)
  {}
  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * SM1 = SM2
   */
  SquareMatrix<Type, M> &operator=(const SquareMatrix<Type, M> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  /*
   * SM1 = M2
   */
  SquareMatrix<Type, M> &operator=(const Matrix<Type, M, M> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * SM1 *= SM2
   */
  void operator*=(const Matrix<Type, M, M> &vb)
  {
    *this = *this * vb;
  }
  
  /*
   * SM1 *= S2
   */
  void operator*=(Type scalar)
  {
    Type *ptr1 = (Type*)this->data;
    
    for(size_t i=0; i<M*M; i++) {
      ptr1[i] *= scalar;
    }
  }
  
  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  SquareMatrix<Type, M> inverse() const
  {
    SquareMatrix<Type, M> vr;
    
    if(!inv(*this, vr)) {
      vr.set_zero();
    }
    
    return vr;
  }
  
  inline SquareMatrix<Type, M> I() const
  {
    return this->inverse(); 
  }
  
  inline bool inverse(SquareMatrix<Type, M> &vr) const
  {
      return inv(*this, vr);
  }
  
  inline bool I(SquareMatrix<Type, M> &vr) const
  {
    return inv(*this, vr);
  }
  
  void set_identity()
  {
    this->set_zero();
    
    for (size_t i = 0; i < M; i++) {
      this->data[i][i] = 1;
    }
  }

  inline void identity()
  {
      set_identity();
  }
  
  Vector<Type, M> diag() const
  {
    Vector<Type, M> vr;
    
    for (size_t i = 0; i < M; i++) {
      vr.data[0][i] = this->data[i][i];
    }
    
    return vr;
  }

  Type trace() const
  {
    Type vr = 0;

    for (size_t i = 0; i < M; i++) {
        vr += this->data[i][i];
    }
    
    return vr;
  }
};


/*---------------------------------------------*
 *               DiagMatrix                  *
 *---------------------------------------------*/
template<typename Type, size_t  M>
class DiagMatrix : public Matrix<Type, M, 1>
{
public:
  
  Type *v = (Type*)this->data;
  
  DiagMatrix() :
    Matrix<Type, M, 1>()
  {}
  
  DiagMatrix(Type data0) :
    Matrix<Type, M, 1>(data0)
  {}
  
  DiagMatrix(const Type *data0) :
    Matrix<Type, M, 1>(data0)
  {}
  
  DiagMatrix(const DiagMatrix<Type, M> &vb) :
    Matrix<Type, M, 1>(vb.data)
  {}
  
  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * DM1 = DM2
   */
  DiagMatrix<Type, M> &operator=(const DiagMatrix<Type, M> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * DM1 = M2
   */
  DiagMatrix<Type, M> &operator=(const Matrix<Type, M, 1> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * DM1 + DM2
   */
  DiagMatrix<Type, M> operator+(const DiagMatrix<Type, M> &vb) const
  {
    DiagMatrix<Type, M> vr;
    
    for (size_t i = 0; i < M; i++) {
      vr.v[i] = v[i] + vb.v[i];
    }
    
    return vr;
  }
  
  /*
   * DM1 + SM2
   */
  SquareMatrix<Type, M> operator+(const SquareMatrix<Type, M> &vb) const
  {
    SquareMatrix<Type, M> vr = vb;

    for (size_t i = 0; i < M; i++) {
      vr.data[i][i] += v[i];
    }
    
    return vr;
  }
  
  /*
   * DM1 - DM2
   */
  DiagMatrix<Type, M> operator-(const DiagMatrix<Type, M> &vb) const
  {
    DiagMatrix<Type, M> vr;
    
    for (size_t i = 0; i < M; i++) {
      vr.v[i] = v[i] - vb.v[i];
    }
    
    return vr;
  }
  
  /*
   * DM1 - SM2
   */
  SquareMatrix<Type, M> operator-(const SquareMatrix<Type, M> &vb) const
  {
    SquareMatrix<Type, M> vr = -vb;

    for (size_t i = 0; i < M; i++) {
      vr.data[i][i] += v[i];
    }
    
    return vr;
  }
  
  /*
   * DM1 * M2
   */
  template<size_t N>
  Matrix<Type, M, N> operator*(const Matrix<Type, M, N> &vb) const
  {
    Matrix<Type, M, N> vr = vb;

    for (size_t i = 0; i < M; i++) {
      for (size_t j = 0; j < N; j++) {
        vr.data[i][j] *= v[i];
      }
    }
    
    return vr;
  }
  
  /*
   * DM1 * S2
   */
  DiagMatrix<Type, M> operator*(Type scalar) const
  {
    DiagMatrix<Type, M> vr;
    
    for (size_t i = 0; i < M; i++) {
      vr.data[i][0] = v[i] * scalar;
    }
    
    return vr;
  }
  
  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  DiagMatrix<Type, M> transpose()
  {
    DiagMatrix<Type, M> vr = *this;
    return vr;
  }
  
  inline DiagMatrix<Type, M> T()
  {
    return transpose();
  }
  
};






/*----------------------------------------*
 * Useful function                        *
 *----------------------------------------*/
template<typename Type, size_t M>
SquareMatrix<Type, M> eye() 
{
  SquareMatrix<Type, M> m;
  m.set_identity();
  return m;
}

template<typename Type, size_t M>
SquareMatrix<Type, M> diag(Vector<Type, M> &vb) 
{
  SquareMatrix<Type, M> m(0);
  for (size_t i = 0; i < M; i++) {
      m.data[i][i] = vb.data[0][i];
  }
  return m;
}

template<typename Type, size_t M>
SquareMatrix<Type, M> expm(const Matrix<Type, M, M> &A, size_t order=5)
{
  SquareMatrix<Type, M> vr;
  SquareMatrix<Type, M> A_pow = A;
  vr.set_identity();
  size_t i_factorial = 1;
  
  for (size_t i = 1; i <= order; i++) {
      i_factorial *= i;
      vr += A_pow / i_factorial;
      A_pow *= A_pow;
  }
  
  return vr;
}


/*
 * inverse based on LU factorization with partial pivotting
 * M>3 ,this function exists bug???
 */
template<typename Type, size_t M>
bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
{
  if(M == 2)
  {
    Type det_inv = 1.0f/(A.data[0][0]*A.data[1][1] - A.data[0][1]*A.data[1][0]);
    if(!isfinite(det_inv))
    {
      return false;
    }
    inv.data[0][0] = A.data[1][1]*det_inv;
    inv.data[0][1] =-A.data[0][1]*det_inv;
    inv.data[1][0] =-A.data[1][0]*det_inv;
    inv.data[1][1] = A.data[0][0]*det_inv;
    return true;
  }
  else if(M == 3)
  {
    SquareMatrix<Type, 3> C;
    
    C.data[0][0] = (A.data[1][1]*A.data[2][2] - A.data[2][1]*A.data[1][2]);
    C.data[0][1] =-(A.data[1][0]*A.data[2][2] - A.data[2][0]*A.data[1][2]);
    C.data[0][2] = (A.data[1][0]*A.data[2][1] - A.data[2][0]*A.data[1][1]);
    C.data[1][0] =-(A.data[0][1]*A.data[2][2] - A.data[2][1]*A.data[0][2]);
    C.data[1][1] = (A.data[0][0]*A.data[2][2] - A.data[2][0]*A.data[0][2]);
    C.data[1][2] =-(A.data[0][0]*A.data[2][1] - A.data[2][0]*A.data[0][1]);
    C.data[2][0] = (A.data[0][1]*A.data[1][2] - A.data[1][1]*A.data[0][2]);
    C.data[2][1] =-(A.data[0][0]*A.data[1][2] - A.data[1][0]*A.data[0][2]);
    C.data[2][2] = (A.data[0][0]*A.data[1][1] - A.data[1][0]*A.data[0][1]);
        
    Type det_inv = 1.0f / (A.data[0][0]*C.data[0][0] + A.data[0][1]*C.data[0][1] + A.data[0][2]*C.data[0][2]);
    
    if(!isfinite(det_inv))
    {
      return false;
    }
    inv = C.T()*det_inv;
    return true;
  }
  else
  {
    SquareMatrix<Type, M> L;
    L.set_identity();
    SquareMatrix<Type, M> U = A;
    SquareMatrix<Type, M> P;
    P.set_identity();

    //printf("A:\n"); A.print();

    // for all diagonal elements
    for (size_t n = 0; n < M; n++) {

        // if diagonal is zero, swap with row below
        if (fabsf(static_cast<float>(U.data[n][n])) < 1e-8f) {
            //printf("trying pivot for row %d\n",n);
            for (size_t i = n + 1; i < M; i++) {

                //printf("\ttrying row %d\n",i);
                if (fabsf(static_cast<float>(U.data[i][n])) > 1e-8f) {
                    //printf("swapped %d\n",i);
                    U.swap_rows(i, n);
                    P.swap_rows(i, n);
                    L.swap_rows(i, n);
                    L.swap_cols(i, n);
                    break;
                }
            }
        }

        // failsafe, return zero matrix
        if (fabsf(static_cast<float>(U.data[n][n])) < 1e-8f) {
            return false;
        }

        // for all rows below diagonal
        for (size_t i = (n + 1); i < M; i++) {
            L.data[i][n] = U.data[i][n] / U.data[n][n];

            // add i-th row and n-th row
            // multiplied by: -a(i,n)/a(n,n)
            for (size_t k = n; k < M; k++) {
                U.data[i][k] -= L.data[i][n] * U.data[n][k];
            }
        }
    }

    //printf("L:\n"); L.print();
    //printf("U:\n"); U.print();

    // solve LY=P*I for Y by forward subst
    //SquareMatrix<Type, M> Y = P;

    // for all columns of Y
    for (size_t c = 0; c < M; c++) {
        // for all rows of L
        for (size_t i = 0; i < M; i++) {
            // for all columns of L
            for (size_t j = 0; j < i; j++) {
                // for all existing y
                // subtract the component they
                // contribute to the solution
                P.data[i][c] -= L.data[i][j] * P.data[j][c];
            }
        }
    }

    // for all columns of X
    for (size_t c = 0; c < M; c++) {
        // for all rows of U
        for (size_t k = 0; k < M; k++) {
            // have to go in reverse order
            size_t i = M - 1 - k;

            // for all columns of U
            for (size_t j = i + 1; j < M; j++) {
                // for all existing x
                // subtract the component they
                // contribute to the solution
                P.data[i][c] -= U.data[i][j] * P.data[j][c];
            }

            // divide by the factor
            // on current
            // term to be solved
            //
            // we know that U(i, i) != 0 from above
            P.data[i][c] *= 1/U.data[i][i];
        }
    }

    //check sanity of vrults
    for (size_t i = 0; i < M; i++) {
        for (size_t j = 0; j < M; j++) {
            if (!isfinite(P.data[i][j])) {
                return false;
            }
        }
    }
    //printf("X:\n"); X.print();
    inv = P;
    return true;
  }
}

/*
 * inverse based on LU factorization with partial pivotting
 */
template<typename Type, size_t M>
SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
{
    SquareMatrix<Type, M> i;
    if(inv(A, i)) {
        return i;
    } else {
        i.set_zero();
        return i;
    }
}

/*
 * cholesky decomposition
 *
 * Note: A must be positive definite
 */
template<typename Type, size_t M>
SquareMatrix<Type, M> cholesky(const SquareMatrix<Type, M> &A)
{
    SquareMatrix<Type, M> L;
    for (size_t j = 0; j < M; j++) {
        for (size_t i = j; i < M; i++) {
            if (i==j) {
                float sum = 0;
                for (size_t k = 0; k < j; k++) {
                    sum += L.data[j][k]*L.data[j][k];
                }
                Type vr = A.data[j][j] - sum;
                if (vr <= 0) {
                    L.data[j][j] = 0;
                } else {
                    L.data[j][j] = sqrtf(vr);
                }
            } else {
                float sum = 0;
                for (size_t k = 0; k < j; k++) {
                    sum += L.data[i][k]*L.data[j][k];
                }
                if (L.data[j][j] <= 0) {
                    L.data[i][j] = 0;
                } else {
                    L.data[i][j] = (A.data[i][j] - sum)/L.data[j][j];
                }
            }
        }
    }
    return L;
}

/**
 * cholesky inverse
 *
 * TODO: Check if gaussian elimination jumps straight to back-substitution
 * for L or we need to do it manually. Will impact speed vbwise.
 */
template<typename Type, size_t M>
SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
{
    SquareMatrix<Type, M> L_inv = inv(cholesky(A));
    return L_inv.T()*L_inv;
}

typedef SquareMatrix<float, 2> SMatrix2f;
typedef SquareMatrix<float, 3> SMatrix3f;
typedef SquareMatrix<float, 4> SMatrix4f;
typedef SquareMatrix<float, 5> SMatrix5f;
typedef SquareMatrix<float, 6> SMatrix6f;

typedef DiagMatrix<float, 2> DMatrix2f;
typedef DiagMatrix<float, 3> DMatrix3f;
typedef DiagMatrix<float, 4> DMatrix4f;
typedef DiagMatrix<float, 5> DMatrix5f;
typedef DiagMatrix<float, 6> DMatrix6f;

} // namespace matrix

using namespace matrix;
